#define _USE_MATH_DEFINES
#include "AlgoRLPrivate.h"
#include <iostream>
#include "reader/NCoreReader.h"
#include "RLPlatformData.h"
#include "RLPlanningEngine.h"
#include "RLGeneralizationEngine.h"
#include "RLResultWriter.h"
#include <cmath>

namespace algo
{
    // Helper function to calculate distance between two coordinates (Haversine formula approximation)
    double calculateDistance(const algobase::CoordZ& c1, const algobase::CoordZ& c2) {
        const double R = 6371e3; // Earth radius in meters
        double lat1 = c1[1] * M_PI / 180.0;
        double lat2 = c2[1] * M_PI / 180.0;
        double delta_lat = (c2[1] - c1[1]) * M_PI / 180.0;
        double delta_lon = (c2[0] - c1[0]) * M_PI / 180.0;

        double a = std::sin(delta_lat / 2) * std::sin(delta_lat / 2) +
                   std::cos(lat1) * std::cos(lat2) *
                   std::sin(delta_lon / 2) * std::sin(delta_lon / 2);
        double c = 2 * std::atan2(std::sqrt(a), std::sqrt(1 - a));
        
        double horizontal_dist = R * c;
        double vertical_dist = c2[2] - c1[2]; // depth difference

        return std::sqrt(horizontal_dist * horizontal_dist + vertical_dist * vertical_dist);
    }

    AlgoRLPrivate::AlgoRLPrivate()
    {
    }

    AlgoRLPrivate::~AlgoRLPrivate()
    {
    }

    int AlgoRLPrivate::exec(const algobase::TaskConfig& taskConfig, const std::map<std::string, darr::DimsArray<float>>& topographyData, const algobase::RLConfig& config, algobase::Paths& paths)
    {
        // 1. Check if topography data is valid
        if (topographyData.find("depth") == topographyData.end() || topographyData.at("depth").isEmpty())
        {
            std::cerr << "Error: 'depth' data not found or is empty in the provided topography data." << std::endl;
            return -1;
        }

        // 2. Initialize RL components
        RLPlatformData platformData;
        RLPlanningEngine planningEngine(platformData, topographyData, config);
        RLGeneralizationEngine generalizationEngine;

        // 3. Run the planning engine
        algobase::Path path = planningEngine.findPath(taskConfig.mStartPos, taskConfig.mEndPos);
        paths.push_back(path);

        // 4. Convert path to trajectory and calculate metrics
        algobase::TrajectoryCoords trajectory;
        if (!path.empty())
        {
            double total_distance = 0.0;
            double total_time = 0.0;
            const double constant_speed = 5.0; // m/s, FIXME: Should get from platform data

            algobase::TrajectoryCoord start_point;
            start_point.mCoordZ = path[0].mCoordZ;
            start_point.mHeading = path[0].mHeading;
            start_point.mSpeed = path[0].mSpeed;
            trajectory.push_back(start_point);

            for (size_t i = 1; i < path.size(); ++i)
            {
                algobase::TrajectoryCoord traj_point;
                traj_point.mCoordZ = path[i].mCoordZ;
                traj_point.mHeading = path[i].mHeading;
                traj_point.mSpeed = path[i].mSpeed;

                double segment_distance = calculateDistance(path[i-1].mCoordZ, path[i].mCoordZ);
                double segment_time = (constant_speed > 0) ? (segment_distance / constant_speed) : 0;

                total_distance += segment_distance;
                total_time += segment_time;

                traj_point.mDistance = segment_distance;
                traj_point.tDistance = total_distance;
                traj_point.mTime = segment_time;
                traj_point.tTime = total_time;
                
                trajectory.push_back(traj_point);
            }
        }

        // 5. Save trajectory to file
        if (RLResultWriter::writeToFile("trajectory_output.csv", trajectory))
        {
            std::cout << "Trajectory successfully written to trajectory_output.csv" << std::endl;
        }
        else
        {
            std::cerr << "Error writing trajectory to file." << std::endl;
        }

        // 6. Print training duration
        std::cout << "RL Training took " << planningEngine.getTrainingDuration() << " seconds." << std::endl;

        return 0;
    }
}